/*
 * matching.cpp
 *
 *  Created on: Apr 27, 2011
 *      Author: tqlong
 */

#include "geometric_problem.h"
#include "matching.h"

void MatchingProblem::loadData(const std::vector<std::string>& fName, bool binary) {
  if (fName.size() < n_data()) {
    Log::m_log << "Please provide at least " << n_data() << " file names"
        << endl;
    return;
  }
  for (int i = 0; i < n_data(); i++)
    common::loadVVec(fName[i], m_point[i], binary);
  if (m_match)
    delete m_match;
  m_match = NULL;
}

void MatchingProblem::solve() {
  createDatasets();
  if (m_match) delete m_match;
  m_match = new Matching(*m_set[0], *m_set[1]);
  m_match = new MaximumMatching(*m_set[0], *m_set[1], 5);
  m_match->doMatch();
}

void MaximumMatchingProblem::solve() {
  createDatasets();
  if (m_match) delete m_match;
  Log::m_log << "radius = " << m_r << endl;
  m_match = new MaximumMatching(*m_set[0], *m_set[1], m_r);
  m_match->doMatch();
}

void MatchingProblem::createDatasets()
{
  if (m_set[0]) delete m_set[0];
  if (m_set[1]) delete m_set[1];
  m_set[0] = new dataset_type(m_point[0],1);
  m_set[1] = new dataset_type(m_point[1],1);
}
//struct BoundingBoxDrawer
//{
//  Plotter& m_pl;
//  BoundingBoxDrawer(Plotter& pl) : m_pl(pl) {}
//  void operator()(Tree* tree_node) {
//    KDTree *node = static_cast<KDTree*>(tree_node);
//    const BoundingBox& b = node->box();
//    m_pl.drawRect(QRectF(m_pl.point(b.min()), m_pl.point(b.max())));
//  }
//};

void MatchingProblem::draw(Plotter& pl) {
  QPen pen(Qt::SolidLine);
  pen.setWidth(2);

  pen.setColor(Qt::blue);
  common::drawVVec(m_point[0], pl, pen);
  pen.setColor(Qt::red);
  common::drawVVec(m_point[1], pl, pen);

  if (!m_match)
    return;
  pen.setStyle(Qt::DashLine);
  pen.setWidth(1);
  pen.setColor(Qt::black);
  pl.setPen(pen);

  for (int i = 0; i < m_match->n(); i++) {
    int j = m_match->match(i);
    if (j >= 0)
      pl.drawLine(pl.point(m_point[0][i]), pl.point(m_point[1][j]));
  }
}

void AuctionMatchingProblem::solve() {
  createDatasets();
  if (m_match) delete m_match;
  m_match = new AuctionMatching(*m_set[0], *m_set[1]);
  m_match->doMatch();
}

void AuctionMatchingKDTreeProblem::solve() {
  createDatasets();
  if (m_match) delete m_match;
  m_match = new AuctionMatchingKDTree(*m_set[0], *m_set[1]);
  m_match->doMatch();
}

// ========= MATCHING ===========

Matching::Matching(const dataset_type & set1, const dataset_type &set2) :
  m_set1(set1), m_set2(set2) {
}

double Matching::weight(int i, int j) const {
  return arma::norm(m_set1[i] - m_set2[j], 2);
}

int Matching::n() const {
  return m_set1.size();
}

int Matching::match(int i) const {
  return i >= m_matchIdx.size() ? -1 : (i < n() ? m_matchIdx[i] - n()
      : m_matchIdx[i]);
}

double Matching::cost(const std::vector<int>& matchIdx) {
  double s = 0;
  for (int i = 0; i < n() && i < matchIdx.size(); i++) {
    int j = matchIdx[i];
    if (j >= 0)
      s += weight(i, j - n());
  }
  return s;
}

double Matching::cost() {
  return cost(m_matchIdx);
}

void Matching::doMatch() {
  if (m_set1.size() != m_set2.size())
    return;
  m_matchIdx = std::vector<int>(2 * n(), -1); // init empty match
  for (int i = 0; i < 2 * n(); i++)
    m_matchIdx[i] = i < n() ? i + n() : i - n();
  double min_cost = cost();

  for (int k = 0; k < 1000; k++) {
    std::vector<int> m = std::vector<int>(2 * n(), -1);
    for (int i = 0; i < n(); i++)
      m[i] = i + n();
    // make a random permutation
    for (int i = 0; i < n() - 1; i++) {
      int j = rand() % (n() - i) + i;
      int tmp = m[i];
      m[i] = m[j];
      m[j] = tmp;
    }
    // complete the matching
    for (int i = 0; i < n(); i++)
      m[m[i]] = i;
    // compare with current min_cost
    double c = cost(m);
    if (c < min_cost) {
      min_cost = c;
      m_matchIdx = m;
    }
  }

  //    while (findNegativeCycle()) updateCycle();
  Log::m_log << "cost=" << min_cost << endl;
}

bool Matching::findNegativeCycle() {
  // init single source
  m_dist = std::vector<double>(2 * n(), POSITIVE_INFINITY);
  m_pred = std::vector<int>(2 * n(), -1);
  m_dist[0] = 0; // 0 is the source

  // repeat relaxing edges
  for (int k = 0; k < 2 * n() - 1; k++) {
    // left to right edges
    for (int i = 0; i < n(); i++) {
      for (int j = n(); j < 2 * n(); j++)
        if (j != m_matchIdx[i]) {
          double d = m_dist[i] - weight(i, j - n());
          if (m_dist[j] > d) {
            m_pred[j] = i;
            m_dist[j] = d;
          }
        }
    }
    // right to left edges
    for (int j = n(); j < 2 * n(); j++) {
      int i = m_matchIdx[j];
      if (i >= 0) {
        double d = m_dist[j] + weight(i, j - n());
        if (m_dist[i] > d) {
          m_pred[i] = j;
          m_dist[i] = d;
        }
      }
    }
  }

  // check negative cycle
  for (int i = 0; i < n(); i++) {
    for (int j = n(); j < 2 * n(); j++)
      if (j != m_matchIdx[i]) {
        double d = m_dist[i] + weight(i, j - n());
        if (m_dist[j] > d) {
          m_cycleStart = j;
          return true;
        }
      }
  }
  // right to left edges
  for (int j = n(); j < 2 * n(); j++) {
    int i = m_matchIdx[j];
    if (i >= 0) {
      double d = m_dist[j] - weight(i, j - n());
      if (m_dist[i] > d) {
        m_cycleStart = j;
        return true;
      }
    }
  }
  return false;
}

void Matching::updateCycle() {
  int j = m_cycleStart;
  std::vector<bool> visited(2 * n(), false);
  // trace the cycle
  do {
    visited[j] = true;
    j = m_pred[j];
  } while (!visited[j]);

  if (j < n())
    j = m_pred[j];
  m_cycleStart = j;
  Log::m_log << "negative cycle: " << m_cycleStart;
  do {
    int i = m_pred[j];
    m_matchIdx[i] = j;
    m_matchIdx[j] = i;
    j = m_pred[i];
    Log::m_log << " --> (i) " << i << " --> (j) " << j;
  } while (j != m_cycleStart);
  Log::m_log << endl;
}

void Matching::outputDistances() const
{
  std::ofstream os("distances.out");
  for (int i = 0; i < n() && i < m_matchIdx.size(); i++) {
    int j = m_matchIdx[i];
    if (j >= 0)
      os << weight(i, j - n()) << std::endl;
  }
  os.close();
}
// =========== KDTreePrice ===============
KDTreePrice::KDTreePrice(const dataset_type& data, std::vector<double>& price) :
  KDTree(data), m_minPrice(NEGATIVE_INFINITY), m_price(price) {
}

KDTreePrice::KDTreePrice(KDTreePrice* parent, int s, int e) :
  KDTree(parent, s, e), m_minPrice(NEGATIVE_INFINITY), m_price(parent->m_price) {
}

Tree* KDTreePrice::createChild(int s, int e) {
  //    Log::m_log << "create child "; Log::m_log.flush();
  return new KDTreePrice(this, s, e);
}

KDTreePrice* KDTreePrice::child(int i) {
  return static_cast<KDTreePrice*> (m_child[i]);
}

KDTreePrice* KDTreePrice::parent() {
  return static_cast<KDTreePrice*> (m_parent);
}

double KDTreePrice::findKNN(const vec& query, std::vector<int>& knnIdx,
    std::vector<double>& minBound) {
  //Log::m_log << "lb=" << m_box.lowerBound(query) << " minP=" << m_minPrice
  //    << " ub=" << minBound.back() << endl;
  if (m_box.lowerBound(query) + m_minPrice >= minBound.back()) {
    return n();
  }
  double pruned = 0;
  if (n_child() == 0) {
    int K = knnIdx.size();
    for_all_node_data(i) {
      double dist = arma::norm(query - get(i), 2) + price(i);
      //Log::m_log << "oi=" << originalIdx(i) << " dist=" << dist << endl;
      for (int k = 0; k < K; k++)
        if (dist < minBound[k]) {
          knnIdx.insert(knnIdx.begin() + k, m_s + i);
          knnIdx.pop_back();
          minBound.insert(minBound.begin() + k, dist);
          minBound.pop_back();
          break;
        }
    }
  } else {
    for_all_node_child(i) {
      pruned += child(i)->findKNN(query, knnIdx, minBound);
    }
  }
  return pruned;
} // findKNN()

double KDTreePrice::price(int idx) const {
  //    Log::m_log << "size=" << m_price.size() << " idx=" << idx << " orig="
  //        << originalIdx(idx) << " price=" << m_price[originalIdx(idx)] << " ";
  //    Log::m_log.flush();
  return m_price[originalIdx(idx)];
}

void KDTreePrice::updateMinPrice(int idx) {
  KDTreePrice *leaf = findLeaf(idx);
  while (leaf) {
    double price = leaf->m_minPrice;
    leaf->updateMinPrice();
    double price_after = leaf->m_minPrice;
    leaf = leaf->parent();
    if (leaf && price > leaf->m_minPrice + 1e-10) // no need to go up the tree
      break;
  }
}

KDTreePrice* KDTreePrice::findLeaf(int idx) {
  for_all_node_child(i) {
    KDTreePrice* c = child(i);
    if (c->s() <= idx && c->e() > idx)
      return c->findLeaf(idx);
  }
  return this;
}

void KDTreePrice::updateMinPrice() {
  double min = POSITIVE_INFINITY;
  if (n_child() == 0) {
    for_all_node_data(i)
      if (price(i) < min)
        min = price(i);
  } else {
    for_all_node_child(i) {
      KDTreePrice* c = child(i);
      if (c->m_minPrice < min)
        min = c->m_minPrice;
    }
  }
  m_minPrice = min;
}

void KDTreePrice::resetPrice() {
  for_all_node_child(i)
    child(i)->resetPrice();
  updateMinPrice();
}

// ========== AuctionMatching ===============
AuctionMatching::AuctionMatching(const dataset_type & set1, const dataset_type &set2) :
  Matching(set1, set2), m_bestIdx(n(), std::vector<int>(2, -1)),
      m_thirdY(n()) {
}

void AuctionMatching::doMatch() {
  m_price = std::vector<double>(n(), 0);
  for (double epsilon = 10; epsilon >= 1.0 / n(); epsilon /= 2) {
    m_matchIdx = std::vector<int>(2 * n(), -1);
    m_count = 0;
    std::queue<int> freeBidder;
    for (int i = 0; i < n(); i++)
      freeBidder.push(i);
    while (!freeBidder.empty()) {
      int i = freeBidder.front(), min_j = -1;
      freeBidder.pop();

      // find v = min w(i,j) + p(j) w = second min w(i,j) + p(j)
      double v = POSITIVE_INFINITY, w = v;
      findBest(i, v, w, min_j);
      //            Log::m_log << "v=" << v << " w=" << w << " min_j=" << min_j << endl;
      if (v == POSITIVE_INFINITY) {
        Log::m_log << "v=INF" << endl;
        exit(0);
      }
      m_price[min_j] += w - v + epsilon;

      int jn = min_j + n();
      int old_i = m_matchIdx[jn];
      m_matchIdx[jn] = i;
      m_matchIdx[i] = jn;
      if (old_i == -1) {
        m_count++;
        if ((n() >= 10 && m_count % (n() / 10) == 0) || (n() < 10))
          Log::m_log << "count=" << m_count << endl;
      } else {
        m_matchIdx[old_i] = -1; // push old_i in the queue
        freeBidder.push(old_i);
      }
    } // while freeBidder not empty
    Log::m_log << "epsilon=" << epsilon << " cost=" << cost() << endl;
  } // for epsilon
  Log::m_log << "final cost=" << cost() << endl;
} // doMatch()

void AuctionMatching::findBest(int i, double& v, double& w, int& min_j) {
  if (canPruned(i, v, w, min_j)) {
    Log::m_log << "idx pruned ";
    Log::m_log.flush();
    return;
  }
  double u = POSITIVE_INFINITY;
  int j1 = -1, j2 = -1;
  for (int j = 0; j < n(); j++) {
    double c = this->weight(i, j) + m_price[j];
    if (c < v) {
      u = w;
      w = v;
      j2 = j1;
      v = c;
      j1 = j;
    } else if (c < w) {
      u = w;
      w = c;
      j2 = j;
    }
  }
  min_j = j1;
  m_bestIdx[i][0] = j1;
  m_bestIdx[i][1] = j2;
  m_thirdY[i] = u;
}

bool AuctionMatching::canPruned(int i, double& v, double& w, int& min_j) {
  int j1 = m_bestIdx[i][0];
  int j2 = m_bestIdx[i][1];
  if (j1 < 0 || j2 < 0)
    return false; // no index recored yet
  double yi = m_thirdY[i];
  double c1 = this->weight(i, j1) + m_price[j1], c2 = this->weight(i, j2)
      + m_price[j2];
  if (c1 <= yi && c2 <= yi) // can pruned
  {
    if (c1 < c2) {
      v = c1;
      w = c2;
      min_j = j1;
    } else {
      v = c2;
      w = c1;
      min_j = j2;
    }
    return true;
  }
  return false;
}

// ========== AuctionMatchingKDTree ============
AuctionMatchingKDTree::AuctionMatchingKDTree(const dataset_type & set1,
    const dataset_type &set2) :
  AuctionMatching(set1, set2), m_tree(new KDTreePrice(m_set2, m_price)) {
  m_tree->buildTree();
  NodeStats<KDTree> v;
  m_tree->visitRecursive(v, false); // post-order visit
}

AuctionMatchingKDTree::~AuctionMatchingKDTree() {
  delete m_tree;
}

void AuctionMatchingKDTree::doMatch() {
  m_price = std::vector<double>(n(), 0);
  m_tree->resetPrice();
  double oldCost = POSITIVE_INFINITY;
  for (double epsilon = 10; epsilon >= 1.0 / n(); epsilon /= 2) {
    m_pruned = 0;
    Log::m_log << epsilon << " pruned=" << m_pruned << " " << endl;
    m_matchIdx = std::vector<int>(2 * n(), -1);
    m_count = 0;
    std::queue<int> freeBidder;
    for (int i = 0; i < n(); i++)
      freeBidder.push(i);
    while (!freeBidder.empty()) {
      int i = freeBidder.front(), min_j = -1;
      freeBidder.pop();

      // find v = min w(i,j) + p(j) w = second min w(i,j) + p(j)
      double v = POSITIVE_INFINITY, w = v;
      findBest(i, v, w, min_j);
      //Log::m_log << "v=" << v << " w=" << w << " min_j=" << min_j << endl;

      if (v == POSITIVE_INFINITY) {
        Log::m_log << "v=INF" << endl;
        exit(0);
      }
      int idx = min_j;
      min_j = m_tree->originalIdx(idx);
      m_price[min_j] += w - v + epsilon;
      m_tree->updateMinPrice(idx);

      int jn = min_j + n();
      int old_i = m_matchIdx[jn];
      m_matchIdx[jn] = i;
      m_matchIdx[i] = jn;
      if (old_i == -1) {
        m_count++;
        if ((n() >= 10 && m_count % (n() / 10) == 0) || (n() < 10))
          Log::m_log << "count=" << m_count << " pruned=" << m_pruned << " "
              << endl;
      } else {
        m_matchIdx[old_i] = -1; // push old_i in the queue
        freeBidder.push(old_i);
      }
    } // while freeBidder not empty
    double newCost = cost();
    double reduction = (oldCost-newCost)/oldCost;
    Log::m_log << "epsilon=" << epsilon << " cost=" << newCost << " reduction=" << reduction << endl;
    if (oldCost != POSITIVE_INFINITY && reduction >= 0 && reduction <= 1e-4) {
      // change too small relatively
      break;
    }
    oldCost = newCost;
  } // for epsilon
  Log::m_log << "final cost=" << cost() << endl;
  outputDistances();
} // doMatch()

void AuctionMatchingKDTree::findBest(int i, double& v, double& w, int& min_j) {
  if (canPruned(i, v, w, min_j)) {
    Log::m_log << "idx pruned ";
    Log::m_log.flush();
    return;
  }
  std::vector<int> knnIdx(3, -1);
  std::vector<double> minBound(3, POSITIVE_INFINITY);
  m_pruned += m_tree->findKNN(m_set1[i], knnIdx, minBound);
  v = minBound[0];
  w = minBound[1];
  min_j = knnIdx[0];

  m_bestIdx[i][0] = knnIdx[0];
  m_bestIdx[i][1] = knnIdx[1];
  m_thirdY[i] = minBound[2];
}

bool AuctionMatchingKDTree::canPruned(int i, double& v, double& w, int& min_j) {
  int jj1 = m_bestIdx[i][0];
  int jj2 = m_bestIdx[i][1];
  if (jj1 < 0 || jj2 < 0)
    return false; // no index recored yet
  int j1 = m_tree->originalIdx(jj1);
  int j2 = m_tree->originalIdx(jj2);
  double yi = m_thirdY[i];
  double c1 = this->weight(i, j1) + m_price[j1], c2 = this->weight(i, j2)
      + m_price[j2];
  if (c1 <= yi && c2 <= yi) // can pruned
  {
    if (c1 < c2) {
      v = c1;
      w = c2;
      min_j = jj1;
    } else {
      v = c2;
      w = c1;
      min_j = jj2;
    }
    return true;
  }
  return false;
}
